
#include "DumperSubsystem.h"
#include "EntechPorts.h"

DumperSubsystem::DumperSubsystem():
	EntechSubsystem("Dumper_Subsystem")
	, m_position(Start)
{
	m_kick = new Solenoid(c_dumperKickChannel);
	m_armUp = new TimedSolenoid(c_dumperArmUpChannel);
	m_armDown = new TimedSolenoid(c_dumperArmDownChannel);
	m_timer = new Timer();
}

DumperSubsystem::~DumperSubsystem()
{
}

bool DumperSubsystem::CommandDone(void)
{
	if ((Low == m_position) && (m_timer->Get() > c_TimeToKick)) {
		return true;
	} else if ((High == m_position) && (m_timer->Get() > c_TimeToMoveArm)) {
		return true;
	} else if (m_position == Start) {
		return true;
	}
	return false;
}

void DumperSubsystem::SetPosition(Position position)
{
	m_position = position;
	m_timer->Stop();
	m_timer->Reset();
	m_timer->Start();
	switch (m_position)
	{
	case Low:
		m_armUp->Trigger(false);
		m_armDown->Trigger(true);
		m_kick->Set(true);
		break;
	case High:
		m_armUp->Trigger(true);
		m_armDown->Trigger(false);
		m_kick->Set(false);
		break;
	case Start:
		m_armUp->Trigger(false);
		m_armDown->Trigger(true);
		m_kick->Set(false);
		break;
	}
}

void DumperSubsystem::DoAutonomousInit(void)
{
	SetPosition(Start);
}

void DumperSubsystem::DoAutonomous(void)
{
	DoKicker();
}

void DumperSubsystem::DoTeleop(void)
{
	DoKicker();
}

void DumperSubsystem::DoTest(void)
{
	
}

void DumperSubsystem::DoKicker(void)
{
	if (Low == m_position) {
		m_kick->Set(true);
	} else {
		m_kick->Set(false);
	}
}
